{

Some Test Program for the robot truck, introduces some simple state based driving.

Version 1.1

Written by Daniel Harris

TODO:
restructure the state system.


}

CON

  'setup the system clock
  '5 MHz input frequency * 16x PLL multiplier = 80 MHz per core
  _clkmode = xtal1 + pll16x
  _xinfreq = 5_000_000


  'input/output pin constant declarations
  'these dont cost anything in the compiled program, similar to
  'C constants

  F_PING_PIN = 0
  R_PING_PIN = 1
  L_PING_PIN = 2
  L45_PING_PIN = 3
  R45_PING_PIN = 4
  BK_PING_PIN = 5
  
  THROTTLE_PIN = 6              
  STEERING_PIN = 7
  RX_STEERING_PIN = 8
  RX_THROTTLE_PIN = 9

  
  'Other constants - makes things easier to read     

  STEER_DELTA = 30
  STEER_CENTER = 1475

  SPEED_STOP    = 1350
  SPEED_SLOW_FWD = 1440          '1403 is threshold between stop and very slow
  SPEED_SLOW_REV = 1300

  'the 1403 comment just above is for my speed controller.  Yours will likely be different.

 
OBJ

  ping:  "Ping"                                         'ultrasonic "ping" sensor driver library.
                                                        'Calls to ping object utilize functions in the Ping
                                                        'library.

  pst    : "Parallax Serial Terminal"                   ' Serial communication object


var

  'Global variable declarations (shared accross all 8 cogs/cores)
  

  long servo_stack[50]
  long sensor_stack[50]
  long display_stack[50]
  

  byte eyes[6]

  long  steering_out, throttle_out

  byte  state, substate, oldstate, status

  word  left_ping, right_ping, front_ping, left_45_ping, right_45_ping, bk_ping

  
PUB Start

  'do any initializations here

  state := 0
  
  cognew(read_sensors, @sensor_stack)
  cognew(run_servos, @servo_stack)
  'cognew(display, @display_stack)         'Visual display of what the pings are seeing, uncomment to activate



  run
                                 

pub run | cur_state_done, future_time, side, temp, steer_change   

  repeat

    case state
      0:  'state 0, no movement, do a quick left/right steering test, wait a second, then go.
        pause(3_000_000)
        steering_out := STEER_CENTER + 400
        pause(500_000)
        steering_out := STEER_CENTER - 400
        pause(500_000)
        steering_out := STEER_CENTER
        pause(1_000_000)
        state := 1
        future_time := cnt
        
      1:  'drive forward and try to avoid walls.
        if(front_ping > 20)     'if we dont see anything, drive forward slowing.
          throttle_out := SPEED_SLOW_FWD
          future_time := Future_cnt(1_000_000)
        else
          if(cnt > future_time) 'if we do see something with the front ping, and its still there for some
                                ' time in the future, then go to backup state (state=2)
            state := 2
          throttle_out := SPEED_STOP  'stop the speed controller and let the truck coast.

        'havent seen anything with the front ping, so do some steering.  
        
        'decide which side is closer to something
        side := (right_ping > left_ping)    'yields a boolean result, store into side
    

        if side                     'check to see which wall is closer
          temp := left_ping          'we are closer (or equal) to the left wall, so look at that sensor's value
        else
          temp := right_ping          'we are closer to the right wall, so look at that sensor's value

        side := ((side&1)<<1)-1         'result of this operation will give us either 1 or -1 from either -1 or 0
        steer_change := 0
 
        if(temp <  40)      'we are less than 40 inches from the wall, take steering action
          steer_change := (40 - temp) * 11     'the closer we are, steer sharper

                                        ' if we want to adjust the steering threshold, change both '40s' above, these
                                        ' need to match for proper math.
     
          steering_out := STEER_CENTER + (-(side)*steer_change)
        else
          steering_out := STEER_CENTER  ' steer straight if we arent close to anything


      2:  'quick and dirty 'back-up-for-2-seconds-or-while-somethings-still-up-front'
        if(left_ping > right_ping)                      'deside which side is closer to something
          steering_out := STEER_CENTER + 400
        else
          steering_out := STEER_CENTER - 400

        throttle_out := SPEED_SLOW_REV
        future_time := Future_cnt(2_000_000)
        repeat until cnt > future_time and front_ping > 50  ' will stop this repeat after future_time is reached
        throttle_out := SPEED_STOP                          ' or if we dont see anything ahead for a ways (50 in.)
        pause(500_000)                                  'wait a moment to let the car slow down
        future_time := cnt
        steering_out := STEER_CENTER                    're-center the steering
        state := 1                                      'forward-ho (hopefully)!
        
        
            



pub display

' A quick and dirty method to give a visual representation of distances received by each ping.
' Displays the sensor and a "#" for each inch, limited to 40

  pst.start(115_200)            'start parallax serial terminal object at 115,200 buad

  pst.clear

  repeat
    pst.str(STRING("L  :"))
    pst.chars(35, eyes[0]<#40)
    pst.chars(32, 40)           'throw spaces on the end of the line, results in clearing any extra chars
    pst.newline
    pst.str(STRING("L45:"))
    pst.chars(35, eyes[1]<#40)
    pst.chars(32, 40)
    pst.NewLine
    pst.str(STRING("F  :"))
    pst.chars(35, eyes[2]<#40)
    pst.chars(32, 40)
    pst.NewLine
    pst.str(STRING("R45:"))
    pst.chars(35, eyes[3]<#40)
    pst.chars(32, 40)
    pst.NewLine
    pst.str(STRING("R  :"))
    pst.chars(35, eyes[4]<#40)
    pst.chars(32, 40)
    pause(50_000)
    pst.home                    ' go back to origin without clearing screen. Looks cleaner this way.
    

pub run_servos

'Uses a cog to constantly address the servo motors.  If we are strapped for cogs later,
'we could throw this in the read sensor cog since that also is running constantly.
'Sticks the values of shared variables throttle_out and steering_out to the servos.
'Update these variables to change what is being output.  Set it and forget it :).

  dira[THROTTLE_PIN]~~          'make pins output
  dira[STEERING_PIN]~~

  steering_out := STEER_CENTER  'set some start parameters
  throttle_out := SPEED_STOP

  repeat                        'do it indefinitely.
  
    pulsout(THROTTLE_PIN, throttle_out)
    pulsout(STEERING_PIN, steering_out)

    pause(20_000)
    



pub read_sensors

'constantly reads the pings in a round robin fasion.  Quick and dirty, this could be
'cleaned up.  For now though, we have PLENTY of available space so we can be loose with
'variables. 
  

  repeat
    front_ping := eyes[2] := ping.inches(F_PING_PIN)
    pause(1_000)
    left_ping := eyes[0] := ping.inches(L_PING_PIN)
    pause(1_000)
    right_ping := eyes[4] := ping.inches(R_PING_PIN)
    pause(1_000)
    left_45_ping := eyes[1] := ping.inches(L45_PING_PIN)
    pause(1_000)
    right_45_ping := eyes[3] := ping.inches(R45_PING_PIN)
    pause(1_000)
    bk_ping := eyes[5] := ping.inches(BK_PING_PIN)
    pause(1_000)
    
  
    

pri Pause(duration)

  'This function pauses program execution for approximately [duration] micro seconds
  'so 1_000_000 would be approximately one second.  Doesnt account for instruction
  'execution time overhead.

  if duration < 381
    duration := 381             'lower bound limit. anything lower than this, we
                                'have to wait until the counter comes back around,
                                'which is MUCH longer than [duration].
  waitcnt(((clkfreq / 1_000_000) * duration) + cnt)


pri Future_cnt(usec)            'simple method to return the count for some usec in the future
  return(((clkfreq / 1_000_000) * usec) + cnt)

  
pri PulsOut(pin, duration)

  'similar to the BS2 command.  This function pulses the [pin] for [duration].
  'This is extremely useful for controlling servos and speed controllers.
  'To constantly make a servo move, you have to constantly be calling this
  'function (I.E. you cant set a value and have the servo stay at that value)

  dira[pin]~~      'make pin an output
  outa[pin]~~      'make pin go high            
  waitcnt(((clkfreq / 1_000_000) * duration) + cnt)  'pause execution
  outa[pin]~       'make pin go low


pri pulsin(pin) : time | cnt1, cnt2

'ruturns the amount of time [pin] goes from low->high->low (the pulse width) in usecs.
'this method blocks, so make sure something is actually on [pin].

  dira[Pin]~                                                                    ' Make I/O Pin Input
  waitpne(0, |< Pin, 0)                                                         ' Wait For Pin To Go HIGH
  cnt1 := cnt                                                                   ' Store Current Counter Value
  waitpeq(0, |< Pin, 0)                                                         ' Wait For Pin To Go LOW 
  cnt2 := cnt 
  time := (||(cnt1 - cnt2) / (clkfreq / 1_000_000))                             'do comparison and return time the pin was high